#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include <filesystem>
#include <csignal>
#include <cstdlib>
#include <fstream>
#include <iomanip>  // For std::put_time
#include <cmath>
#include <memory>
#include <algorithm>


#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/utils.h"
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
#include "nav_2d_msgs/msg/twist2_d.hpp"
#include "common_lib/glog/glog.h"

using namespace std;
using namespace std::chrono_literals;

class TestTrajectory : public rclcpp::Node{
    public:
        TestTrajectory();
        ~TestTrajectory() = default;

        // 获取时间步长
        std::vector<double> getTimeSteps(
            const nav_2d_msgs::msg::Twist2D & cmd_vel);
        // 计算新位姿
        geometry_msgs::msg::Pose2D computeNewPosition(
            const geometry_msgs::msg::Pose2D start_pose,
            const nav_2d_msgs::msg::Twist2D & vel, const double dt);
        // 计算前向距离位姿
        geometry_msgs::msg::Pose2D getForwardPose(
            const geometry_msgs::msg::Pose2D & pose, double distance);
        // 调整路径分辨率 原理为根据大于2倍分辨率距离判断，是否进行线性插值
        vector<geometry_msgs::msg::Pose2D> adjustPlanResolution(
          const vector<geometry_msgs::msg::Pose2D> & global_plan_in,
          double resolution);

    private:
        // 发布原始路径
        rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_path_;
        // 发布前向路径
        rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_forward_path_;

        double sim_time_;
        bool discretize_by_time_;
        double time_granularity_;
        double linear_granularity_;
        double angular_granularity_;
        double forward_point_distance_;
};


TestTrajectory::TestTrajectory(): Node("test_trajectory"){
    LOG(INFO) << "test_trajectory init.";
    // 锁存发布
    auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local();
    pub_path_ = this->create_publisher<nav_msgs::msg::Path>("/path_raw", qos);
    pub_forward_path_ = this->create_publisher<nav_msgs::msg::Path>("/forward_path", qos);


    sim_time_ = 1.7;
    discretize_by_time_ = false;
    time_granularity_ = 0.5;
    linear_granularity_ = 0.5;
    angular_granularity_ = 0.025;
    forward_point_distance_ = 0.325;

    // 生成原始轨迹
    geometry_msgs::msg::Pose2D start_pose;
    nav_2d_msgs::msg::Twist2D start_vel;
    nav_2d_msgs::msg::Twist2D cmd_vel;
    cmd_vel.x = 1.0;
    cmd_vel.theta = 0.2;

    vector<geometry_msgs::msg::Pose2D> traj_raw;
    std::vector<double> steps = getTimeSteps(cmd_vel);
    LOG(INFO) << "steps size: " << steps.size();
    traj_raw.emplace_back(start_pose);
    geometry_msgs::msg::Pose2D pose = start_pose;
    for (double dt : steps) {
        pose = computeNewPosition(pose, cmd_vel, dt);
        traj_raw.emplace_back(pose);
    }

    // 发布原始轨迹
    nav_msgs::msg::Path path_raw;
    path_raw.header.stamp = this->now();
    path_raw.header.frame_id = "map";
    for (auto & pose : traj_raw) {
        geometry_msgs::msg::PoseStamped pose_stamped;
        pose_stamped.header.stamp = this->now();
        pose_stamped.header.frame_id = "map";
        pose_stamped.pose.position.x = pose.x;
        pose_stamped.pose.position.y = pose.y;
        tf2::Quaternion q;
        q.setRPY(0, 0, pose.theta); 
        pose_stamped.pose.orientation = tf2::toMsg(q);
        path_raw.poses.push_back(pose_stamped);
    }
    pub_path_->publish(path_raw);


    // 生成前向轨迹 并发布
    vector<geometry_msgs::msg::Pose2D> traj_forward_raw;
    nav_msgs::msg::Path path_forward;
    path_forward.header.stamp = this->now();
    path_forward.header.frame_id = "map";
    for (auto & pose : traj_raw) {
        geometry_msgs::msg::Pose2D forward_pose 
            = getForwardPose(pose, forward_point_distance_);
        geometry_msgs::msg::PoseStamped pose_stamped;
        pose_stamped.header.stamp = this->now();
        pose_stamped.header.frame_id = "map";
        pose_stamped.pose.position.x = forward_pose.x;
        pose_stamped.pose.position.y = forward_pose.y;
        tf2::Quaternion q;
        q.setRPY(0, 0, forward_pose.theta); 
        pose_stamped.pose.orientation = tf2::toMsg(q);
        path_forward.poses.push_back(pose_stamped);
    }
    pub_forward_path_->publish(path_forward);
}


// 获取时间步长
std::vector<double> TestTrajectory::getTimeSteps(
  const nav_2d_msgs::msg::Twist2D & cmd_vel)
{
  std::vector<double> steps;
  if (discretize_by_time_) {
    steps.resize(ceil(sim_time_ / time_granularity_));
  } else {  // discretize by distance
    double vmag = hypot(cmd_vel.x, cmd_vel.y);

    // the distance the robot would travel in sim_time if it did not change velocity
    double projected_linear_distance = vmag * sim_time_;

    // the angle the robot would rotate in sim_time
    double projected_angular_distance = fabs(cmd_vel.theta) * sim_time_;

    // Pick the maximum of the two
    int num_steps = ceil(
      std::max(
        projected_linear_distance / linear_granularity_,
        projected_angular_distance / angular_granularity_));
    steps.resize(num_steps);
  }
  if (steps.size() == 0) {
    steps.resize(1);
  }
  std::fill(steps.begin(), steps.end(), sim_time_ / steps.size());
  return steps;
}

// 计算新位姿
geometry_msgs::msg::Pose2D TestTrajectory::computeNewPosition(
  const geometry_msgs::msg::Pose2D start_pose,
  const nav_2d_msgs::msg::Twist2D & vel, const double dt)
{
  geometry_msgs::msg::Pose2D new_pose;
  // cos(π/2+α) = -sinα
  new_pose.x = start_pose.x +
    (vel.x * cos(start_pose.theta) + vel.y * cos(M_PI_2 + start_pose.theta)) * dt;
  // sin(π/2+α) = cosα
  new_pose.y = start_pose.y +
    (vel.x * sin(start_pose.theta) + vel.y * sin(M_PI_2 + start_pose.theta)) * dt;
  new_pose.theta = start_pose.theta + vel.theta * dt;
  return new_pose;
}

// 计算前向距离位姿
geometry_msgs::msg::Pose2D TestTrajectory::getForwardPose(
    const geometry_msgs::msg::Pose2D & pose, double distance)
{
  geometry_msgs::msg::Pose2D forward_pose;
  forward_pose.x = pose.x + distance * cos(pose.theta);
  forward_pose.y = pose.y + distance * sin(pose.theta);
  forward_pose.theta = pose.theta;
  return forward_pose;
}


// 调整路径分辨率 原理为根据大于2倍分辨率距离判断，是否进行线性插值
vector<geometry_msgs::msg::Pose2D> TestTrajectory::adjustPlanResolution(
  const vector<geometry_msgs::msg::Pose2D> & global_plan_in,
  double resolution){
  vector<geometry_msgs::msg::Pose2D> global_plan_out;
  if (global_plan_in.size() == 0) {
    return global_plan_out;
  }

  geometry_msgs::msg::Pose2D last = global_plan_in[0];
  global_plan_out.emplace_back(last);

  // we can take "holes" in the plan smaller than 2 grid cells (squared = 4)
  double min_sq_resolution = resolution * resolution * 4.0;

  for (unsigned int i = 1; i < global_plan_in.size(); ++i) {
    geometry_msgs::msg::Pose2D loop = global_plan_in[i];
    double sq_dist = (loop.x - last.x) * (loop.x - last.x) + (loop.y - last.y) * (loop.y - last.y);
    if (sq_dist > min_sq_resolution) {
      // add points in-between
      double diff = sqrt(sq_dist) - sqrt(min_sq_resolution);
      int steps = static_cast<int>(diff / resolution) - 1;
      double steps_double = static_cast<double>(steps);

      double delta_x = (loop.x - last.x) / steps_double;
      double delta_y = (loop.y - last.y) / steps_double;
      double delta_t = (loop.theta - last.theta) / steps_double;

      for (int j = 1; j < steps; ++j) {
        geometry_msgs::msg::Pose2D pose;
        pose.x = last.x + j * delta_x;
        pose.y = last.y + j * delta_y;
        pose.theta = last.theta + j * delta_t;
        global_plan_out.push_back(pose);
      }
    }
    global_plan_out.push_back(global_plan_in[i]);
    last.x = loop.x;
    last.y = loop.y;
  }
  return global_plan_out;
}

int main(int argc, char * argv[]){
    FLAGS_colorlogtostderr = true;   // log信息区分颜色
    FLAGS_logtostderr = 1;           // 允许log信息打印到屏幕
    google::SetStderrLogging(google::GLOG_INFO);  // 输出log的最低等级是 INFO (可以设置为WARNING或者更高)
    google::InstallFailureSignalHandler();     // 配置安装程序崩溃失败信号处理器
    // google::InstallFailureWriter(&signalHandler);  // 安装配置程序失败信号的信息打印过程，设置回调函数
    google::InitGoogleLogging((const char *)argv[0]);  // 用当前可执行程序初始化glog


    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<TestTrajectory>());
    rclcpp::shutdown();
    return 0;
}